#include <memory>
#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/system/error_code.hpp>
#include <boost/system/system_error.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <stdlib.h>

#include <stdio.h>


#define SERIAL_HEAD_A 0xFF
#define SERIAL_HEAD_B 0x55

struct RX_PROTOCOL{
    uint8_t Header1;
	uint8_t Header2;
	int16_t  X_Angle_Speed;
	int16_t  Y_Angle_Speed;
	int16_t  Z_Angle_Speed;
	int16_t  X_Acceleration;
	int16_t  Y_Acceleration;
	int16_t  Z_Acceleration;
	int16_t  Odom[3];
    int16_t  Speed[2];
	int32_t  L_Motor_Speed;
	int32_t  R_Motor_Speed;
	int16_t  L_Motor_Position[2];
	int16_t  R_Motor_Position[2];
	uint8_t  Drop_Sensor[6];
	int16_t  L_Motor_Temperature;
	int16_t  R_Motor_Temperature;
	int16_t  L_Motor_Current;
	int16_t  R_Motor_Current;
	int16_t  R_Motor_Error_Code;		
	int16_t  L_Motor_Error_Code;	
	int16_t  Charge_Current;
	int16_t  Battery_Current;
	int16_t  Battery_Voltage;
	uint16_t Robot_Status;
	uint8_t  Check_Sum;
};

enum packetFinderState
{
    waitingForHead1,
    waitingForHead2,
    waitingForPayload,
    waitingForCheckSum,
    handlePayload
};



typedef boost::shared_ptr<boost::asio::serial_port> serialp_ptr;
 boost::system::error_code ec_;

packetFinderState state_ = waitingForHead1;

struct RX_PROTOCOL rx_data;

boost::asio::io_service io_service_;

boost::mutex mutex_;

serialp_ptr sp_;

void check_sum(uint8_t* data, size_t len, uint8_t& dest)
{
    dest = 0x00;
	for(int i=0;i<len;i++)
	{
		dest += *(data+i);
	}
}

void distribute_data(uint8_t* buffer)
{
    memcpy(&rx_data,buffer,sizeof(rx_data));
    printf("-------------RX DATA-------------\r\n");
    printf("X Angle Speed  : %3.3f rad/s2 \r\n",((float)rx_data.X_Angle_Speed/32768*1000)/180*3.1415926);
    printf("Y Angle Speed  : %3.3f rad/s2\r\n",((float)rx_data.Y_Angle_Speed/32768*1000)/180*3.1415926);
    printf("Z Angle Speed  : %3.3f rad/s2\r\n",((float)rx_data.Z_Angle_Speed/32768*1000)/180*3.1415926);
    printf("X ACC          : %3.3f m/s2\r\n",((float)rx_data.X_Acceleration/32768*2)*9.8);
    printf("Y ACC          : %3.3f m/s2\r\n",((float)rx_data.Y_Acceleration/32768*2)*9.8);
    printf("Z ACC          : %3.3f m/s2\r\n",((float)rx_data.Z_Acceleration/32768*2)*9.8);
    printf("Odom           : X: %3.3fm  Y: %3.3fm  Z: %3.3frad\r\n",(float)rx_data.Odom[0]/1000,(float)rx_data.Odom[1]/1000,(float)rx_data.Odom[2]/1000);
    printf("Robot Speed    : Linear: %3.3f m/s   Angular: %3.3f rad/s \r\n", (float)rx_data.Speed[0]/1000, (float)rx_data.Speed[1]/1000);
    printf("Motor Speed    : L : %3.3frpm  R: %3.3frpm\r\n", (float)(rx_data.L_Motor_Speed)/1000,  (float)(rx_data.R_Motor_Speed)/1000);
    printf("Motor Position : L : %5d    R : %5d\r\n",rx_data.L_Motor_Position[0]<<16|rx_data.L_Motor_Position[1],rx_data.R_Motor_Position[0]<<16|rx_data.R_Motor_Position[1]);
    printf("Motor Temp     : L : %3dC    R : %3dC\r\n",rx_data.L_Motor_Temperature, rx_data.R_Motor_Temperature);
    printf("Motor Current  : L : %3.3fA   R : %3.3fA\r\n", (float)rx_data.L_Motor_Current*15/1.414/2048,(float)rx_data.R_Motor_Current*15/1.414/2048);
    printf("Motor ErrCode  : L : %4X    R : %4X\r\n", rx_data.L_Motor_Error_Code,rx_data.R_Motor_Error_Code);
    printf("Charge Current : %3.3fA \r\n",(float)rx_data.Charge_Current/1000);
    printf("Bat_Current    : %3.3fA \r\n",(float)rx_data.Battery_Current/1000);
    printf("Bat_Voltage    : %3.3fV \r\n",(float)rx_data.Battery_Voltage/1000);
    printf("\r\n");
    printf("Robot Status   : %4X\r\n",rx_data.Robot_Status);
    printf("\r\n");
    printf("Drop Sense   :     %d     %d   \r\n", rx_data.Drop_Sensor[0], rx_data.Drop_Sensor[1]);
    printf("\r\n");
    printf("                  %d       %d   \r\n", rx_data.Drop_Sensor[2], rx_data.Drop_Sensor[3]);
    printf("\r\n");
    printf("                   %d     %d   \r\n", rx_data.Drop_Sensor[4], rx_data.Drop_Sensor[5]);
    
    
}

int main(int argc, char ** argv)
{

  ros::init(argc, argv, "raymo_sample");
  ros::NodeHandle nh_;
  ros::NodeHandle nh_p_("~");
  
  uint8_t payload_size, check_num, buffer_data[255],payload_type;
  state_ = waitingForHead1;

  
    
  if(sp_)
    {
        ROS_INFO("The SerialPort is already opened!\r\n");
        return false;
    }
    sp_ = serialp_ptr(new boost::asio::serial_port(io_service_));
    sp_->open("/dev/ttyUSB0", ec_);
    if(ec_)
     {
        std::cout<<"error : port_->open() failed...port_name= ttyUSB0" << ", e=" << ec_.message().c_str() <<std::endl;
        return 1;
     }
    sp_->set_option(boost::asio::serial_port_base::baud_rate(115200));
    sp_->set_option(boost::asio::serial_port_base::character_size(8));
    sp_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
    sp_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
    sp_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
    
    while(ros::ok())
    {
	switch (state_)
        {
            case waitingForHead1:
            
                check_num = 0x00;
                boost::asio::read(*sp_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_);
                state_ = buffer_data[0] == SERIAL_HEAD_A ? waitingForHead2 : waitingForHead1;
                break;
            case waitingForHead2:
          
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[1],1),ec_);
                
                state_ = buffer_data[1] == SERIAL_HEAD_B ? waitingForPayload : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_DEBUG("recv head2 error : %02X \r\n",buffer_data[1]);
                }
                break;
            case waitingForPayload:
            
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[2],64),ec_);
                state_ = waitingForCheckSum;
                break;
            case waitingForCheckSum:
                
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[66],1),ec_);
                check_sum(buffer_data,66,check_num);
                state_ = buffer_data[66] == check_num ? handlePayload : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_DEBUG("check sum error : %02X  cala is %02x\r\n",buffer_data[66],check_num);
                }
                break;
            case handlePayload:
                distribute_data(buffer_data);
                state_ = waitingForHead1;
                break;
            default:
                state_ = waitingForHead1;
                break;
        }
    }
}